/**
 * @file ArmorTrackerBase.cpp
 * @brief
 * @author LiuZhihao (2488288779@qq.com)
 * @date 2023-12-10
 *
 * @copyright Copyright (C) 2023, HITCRT_VISION, all rights reserved.
 *
 * @par 修改日志:
 * <table>
 * <tr><th>Date       <th>Author  <th>Description
 * <tr><td>2023-12-10 <td>LiuZhihao  <td>
 * </table>
 */
#include "ArmorTrackerBase.h"

namespace hitcrt {

ArmorTrackerBase::ArmorTrackerBase(const uint maxTrackSize,
                                   const uint maxArmorSize,
                                   std::vector<std::shared_ptr<Track>> &tracks)
    : m_maxTrackSize(maxTrackSize),
      m_tracks(tracks),
      m_maxArmorSize(maxArmorSize) {
    for (int i = 0; i < m_maxTrackSize; i++) {
        m_tracks.push_back(
            std::shared_ptr<hitcrt::Track>(new hitcrt::Track(5)));
    }
}
void ArmorTrackerBase::apply(const TimePoint detectTime,
                             std::vector<Armor> &armors) {
    m_armorIsUsed = std::vector<int>(armors.size(), 0);
    setDetectTime(detectTime);
    // 匹配，返回一个tuple，里面包含三个vector，分别存储匹配上的装甲板下标，没匹配上的装甲板下标，也可做成成员变量传递
    auto matchResult = associate(armors);
    // 更新-> update();功能变为判断轨迹找到/丢失/维持
    update(matchResult, armors);

    kalmanFilter();
}
void ArmorTrackerBase::setDetectTime(const TimePoint detectTime) {
    m_detectTime = detectTime;
}
/**
 * @brief 处理得到装甲板匹配ID，得到轨迹和
 * @param[in] armors        装甲板s
 * @return std::map<uint, uint>
 * @author youngX (yangxing123456789@foxmail.com)
 */
std::map<uint, uint> ArmorTrackerBase::associate(std::vector<Armor> &armors) {
    std::map<uint, uint> armorTrjID;  // using ArmorId as primary key;
    for (int i = 0; i < static_cast<int>(m_tracks.size()); i++) {
        // 跳过空轨迹
        if (m_tracks[i]->m_trjState == TrjState::INIT) {
            continue;
        }
        // 在新装甲板中找到与该轨迹最匹配的
        int bestArmorID = matchImpl(m_tracks[i], armors);
        if (bestArmorID != -1) {
            m_armorIsUsed[bestArmorID] = 1;

            // 匹配到的装甲板索引号和轨迹序列号构建对组
            std::pair<uint, uint> newPair(bestArmorID, i);
            armorTrjID.insert(newPair);

            // 保证轨迹的装甲板序列长度阈值
            if (m_tracks[i]->m_armors.size() >= m_maxArmorSize) {
                m_tracks[i]->m_armors.pop();
            }

            // 塞入上一个装甲板
            m_tracks[i]->m_lastPoint =
                m_tracks[i]->m_armors.back().m_measurePoint;
            m_tracks[i]->m_armors.push(armors[bestArmorID]);
            // 塞入新一个装甲板
            m_tracks[i]->m_newPoint =
                m_tracks[i]->m_armors.back().m_measurePoint;

            // 这里不对轨迹的状态判断 即滤波的时候仅仅会对 Updated 的装甲板滤波
            // 无需考虑其他的

            m_tracks[i]->m_horizontalDistance =
                armors[bestArmorID].m_horizontalDistance;
            m_tracks[i]->m_calcPitchRAD = armors[bestArmorID].m_calcPitchRAD;
            m_tracks[i]->m_calcYawRAD = armors[bestArmorID].m_calcYawRAD;

            // 统计轨迹中数字最多的装甲板，提高数字识别准确率
            m_tracks[i]->m_foundNums[armors[bestArmorID].m_pattern]++;
            m_tracks[i]->m_trjState = TrjState::NORMAL;
        }
    }
    // 没匹配到的装甲板创建新轨迹
    for (int i = 0; i < static_cast<int>(armors.size()); i++) {
        // 多了一个判断防止因为装甲板数量过多导致新创建轨迹
        // std::cout<<i<<std::endl;
        if (armorTrjID.count(i) == 0 && armors.size() < m_maxTrackSize) {
            int trjId = createTracks(armors[i], m_detectTime);
            // 如果发现装甲板没被匹配到 但是总数量又小于最大trace数量 会返回-1
            //-1是不能被转为unit类型的 所以在这里需要判断一下
            if (trjId != -1) {
                std::pair<uint, uint> newPair(i, trjId);
                armorTrjID.insert(newPair);
            }
        }
    }
    return armorTrjID;
}
int ArmorTrackerBase::matchImpl(std::shared_ptr<Track> &trj,
                                std::vector<Armor> &armors) {
    // TODO

    // 具体匹配可大致分为两部分
    // 1 装甲板固有属性匹配 宽长比 灯条亮度
    // 2 装甲板运动属性 速度（上一个装甲板运动速度
    // 和上上一个装甲板运动速度（包括方向
    // 方向和速度应该作为两个单独的评价指标）的加权求和）两个装甲板的空间欧式距离
    double valueHigh = -100;
    int bestID;
    for (int i = 0; i < armors.size(); i++) {
        // 1创建评价指标
        // 宽高比
        double wdhToHghEvalution =
            std::fabs(armors[i].m_aspectRatio -
                      trj->m_armors.back().m_aspectRatio) /
            trj->m_armors.back().m_aspectRatio;
        // 欧氏距离
        double distanceEvalution =
            -std::abs((std::sqrt(std::pow(armors[i].m_centerUV.x -
                                              trj->m_armors.back().m_centerUV.x,
                                          2) +
                                 std::pow(armors[i].m_centerUV.y -
                                              trj->m_armors.back().m_centerUV.y,
                                          2)) -
                       trj->m_distanceThresh) /
                      trj->m_distanceThresh) /
            10;

        // 装甲板数字
        double patternEvalution = armors[i].m_pattern == trj->m_pattern ? 1 : 0;
        // 2填充评价计算
        // 3分配权重
        double valueTemp = distanceEvalution * 0.4 + wdhToHghEvalution * 0.2 +
                           patternEvalution * 0.4;
        bestID = valueTemp > valueHigh ? i : bestID;
        valueHigh = valueHigh > valueTemp ? valueHigh : valueTemp;
        // std::cout<<"distance: "<<distanceEvalution<<std::endl;
        // std::cout<<"widthtoHeight: "<<wdhToHghEvalution<<std::endl;
        // std::cout<<"pattern: "<<patternEvalution<<std::endl;
        // //计算最优选择
        // std::cout<<"valueTemp: "<<valueTemp<<std::endl;
    }
    // std::cout<<"bestId "<<bestID<<std::endl;
    // std::cout<<"valueHigh: "<<valueHigh<<std::endl;
    if (valueHigh < -1) {
        bestID = -1;
    }
    return bestID;
}
int ArmorTrackerBase::createTracks(Armor _armor, const TimePoint detectTime) {
    // 遍历轨迹若有空轨迹（m_null=true）则创建新轨迹
    // 即利用前面清空的轨迹创建新轨迹
    for (int i = 0; i < static_cast<int>(m_maxTrackSize); i++) {
        if (m_tracks[i]->m_trjState == TrjState::INIT) {
            m_tracks[i]->reset(_armor, detectTime);  // 创建轨迹
            return i;
        }
    }
    return -1;
}
void ArmorTrackerBase::update(std::map<uint, uint> &matchResult,
                              const std::vector<Armor> &armors) {
    for (int i = 0; i < static_cast<int>(armors.size()); i++) {
        std::map<uint, uint>::iterator isFind = matchResult.find(i);
        if (isFind == matchResult.end()) {
            continue;
        }

        int trjID = isFind->second;
        m_tracks[trjID]->m_findCnt++;

        m_tracks[trjID]->m_missCnt = 0;
        m_tracks[trjID]->m_total++;

        m_tracks[trjID]->updateDetectTime(m_detectTime);

        m_tracks[trjID]->m_isUpdated = true;

        int maxSize = 0, maxSizeNum = 0;

        for (int j = 0;
             j < static_cast<int>(m_tracks[trjID]->m_foundNums.size()); j++) {
            if (m_tracks[trjID]->m_foundNums[j] > maxSize) {
                maxSize = m_tracks[trjID]->m_foundNums[j];
                maxSizeNum = j;
            }
        }

        if (((double)maxSize / (double)m_tracks[trjID]->m_total) - 0.5 >
                0.001 &&
            maxSizeNum != 0) {
            m_tracks[trjID]->m_pattern = (Pattern)maxSizeNum;
        }
        if (m_tracks[trjID]->m_armors.back().m_pattern != (Pattern)maxSizeNum) {
            m_tracks[trjID]->m_errCnt += 1;
        } else {
            m_tracks[trjID]->m_errCnt = 0;
        }
    }
    for (int i = 0; i < static_cast<int>(m_tracks.size()); i++) {
        if (m_tracks[i]->m_trjState == TrjState::INIT) {
            m_tracks[i]->updateDetectTime(m_detectTime);
            continue;
        }
        if (m_tracks[i]->m_isUpdated == true) {
            m_tracks[i]->m_isUpdated = false;
            // if (m_tracks[i].m_errCnt >= 10)
            //     m_tracks[i].clear();
        } else {
            m_tracks[i]->updateDetectTime(m_detectTime);
            if (m_tracks[i]->m_missCnt >= m_tracks[i]->m_missCntThresh &&
                m_tracks[i]->m_missCnt <= m_tracks[i]->m_clearThresh) {
                m_tracks[i]->m_missCnt++;
                m_tracks[i]->m_trjState = TrjState::FADE;
            } else if (m_tracks[i]->m_missCnt > m_tracks[i]->m_clearThresh) {
                m_tracks[i]->clear();
                m_tracks[i]->m_trjState = TrjState::INIT;
            } else {
                m_tracks[i]->m_missCnt++;
                m_tracks[i]->m_trjState = TrjState::TWINKLE;
            }
        }
    }
}
/**
 * @brief 对updated 轨迹进行滤波
 * @author fx_03 (18249293810@qq.com)
 */
void ArmorTrackerBase::kalmanFilter() {
    for (int i = 0; i < m_tracks.size(); i++) {
        // 先只对Updated这种轨迹滤波
        // 对于刚刚初始化的轨迹在reset里已经进行初始化 不需要执行滤波操作

        if (m_tracks[i]->m_trjState == NORMAL) {
            // 计算时间差 如果其他地方计算过 则可以直接替代
            auto duration =
                std::chrono::duration_cast<std::chrono::milliseconds>(
                    m_tracks[i]->m_thisDetectTime -
                    m_tracks[i]->m_lastDetectTime);

            double deltaTimedelt = duration.count() / 1000.0;
            std::vector<double> deltaTime;   // 滤波相差时间 deltaTime
            std::vector<double> measureCov;  // 测量误差矩阵 measureCov
            std::vector<double>
                measure;  // 传感器测量数据 这里为点的坐标 measure
            deltaTime.emplace_back(deltaTimedelt);
            measureCov.emplace_back(10.);  // 不知道 先塞0

            cv::Point3f tempPoint(m_tracks[i]->m_armors.back().m_measurePoint);

            measure.emplace_back(tempPoint.x);
            measure.emplace_back(tempPoint.y);
            measure.emplace_back(tempPoint.z);

            // 打包数据 {时间 误差 坐标 xyz}

            std::vector<boost::any> param;
            param.push_back(deltaTimedelt);  // int
            param.push_back(measureCov);     // double
            param.push_back(measure);

            // std::cout <<  "滤波前" <<
            // m_tracks[i]->m_armors.back().m_measurePoint
            //       << std::endl;
            // KalmanFilter 轨迹更新 --> 每个KFC轨迹更新
            m_tracks[i]->m_kf1->updated(m_tracks[i]->m_lastPoint,
                                        m_tracks[i]->m_newPoint, param);
            m_tracks[i]->m_kf1->getResult(
                m_tracks[i]->m_armors.back().m_filterPointmeasure);
        }
    }
}
}  // namespace hitcrt